/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef DWA_LOCAL_PLANNER_DWA_PLANNER_H_
#define DWA_LOCAL_PLANNER_DWA_PLANNER_H_

#include <vector>
#include <Eigen/Core>
#include <dwa_local_planner/DWAPlannerConfig.h>
//for creating a local cost grid
#include <base_local_planner/map_grid_visualizer.h>
//for obstacle data access
#include <costmap_2d/costmap_2d.h>
#include <base_local_planner/trajectory.h>
#include <base_local_planner/local_planner_limits.h>
#include <base_local_planner/local_planner_util.h>
#include <base_local_planner/simple_trajectory_generator.h>
#include <base_local_planner/oscillation_cost_function.h>
#include <base_local_planner/map_grid_cost_function.h>
#include <base_local_planner/obstacle_cost_function.h>
#include <base_local_planner/twirling_cost_function.h>
#include <base_local_planner/simple_scored_sampling_planner.h>
#include <nav_msgs/Path.h>

namespace dwa_local_planner
{
/**
 * @class DWAPlanner 实现DWA算法的局部规划器类
 * @brief A class implementing a local planner using the Dynamic Window Approach
 */
class DWAPlanner
{
public:
    /**
     * @brief  Constructor for the planner 规划器的构造
     * @param name The name of the planner 规划器的名字
     * @param costmap_ros A pointer to the costmap instance the planner should use 规划器的将用到的代价地图实例指针
     * @param global_frame the frame id of the tf frame to use  用到的tf frame 中的frame id
     */
    DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util);

    /**
     * @brief Reconfigures the trajectory planner 配置局部规划器
     */
    void reconfigure(DWAPlannerConfig &cfg);

    /** 对于位置/速度组合，轨迹是否合法
     * @brief  Check if a trajectory is legal for a position/velocity pair
     * @param pos The robot's position 机器人的位置
     * @param vel The robot's velocity 机器人的速度
     * @param vel_samples The desired velocity 期望速度
     * @return True if the trajectory is valid, false otherwise 如果是True, 轨迹合法
     */
    bool checkTrajectory(const Eigen::Vector3f pos, const Eigen::Vector3f vel, const Eigen::Vector3f vel_samples);

    /** 从机器人当前的位置和速度找出最好的轨迹去执行
     * @brief Given the current position and velocity of the robot, find the best trajectory to exectue
     * @param global_pose The current position of the robot 机器人的位置
     * @param global_vel The current velocity of the robot  机器人的速度
     * @param drive_velocities The velocities to send to the robot base 给机器人底座的速度
     * @return The highest scoring trajectory. A cost >= 0 means the trajectory is legal to execute. 返回最高得分的轨迹, cost >= 0 意味着该轨迹可以被执行
     */
    base_local_planner::Trajectory findBestPath(
            const geometry_msgs::PoseStamped &global_pose,
            const geometry_msgs::PoseStamped &global_vel,
            geometry_msgs::PoseStamped &drive_velocities);

    /** 在路径规划前更新代价函数
     * @brief  Update the cost functions before planning
     * @param  global_pose The robot's current pose 机器人的位置
     * @param  new_plan The new global plan 新的规划路径
     * @param  footprint_spec The robot's footprint 机器人的footprint(机器人的底盘形状)
     *
     * The obstacle cost function gets the footprint.
     * The path and goal cost functions get the global_plan
     * The alignment cost functions get a version of the global plan
     *   that is modified based on the global_pose
     */
    void updatePlanAndLocalCosts(const geometry_msgs::PoseStamped &global_pose,
                                 const std::vector<geometry_msgs::PoseStamped> &new_plan,
                                 const std::vector<geometry_msgs::Point> &footprint_spec);

    /** 获得局部规划器预期所需要的计算时间
     * @brief Get the period at which the local planner is expected to run
     * @return The simulation period
     */
    double getSimPeriod() { return sim_period_; }

    /** 计算地图网格单元的代价
     * @brief Compute the components and total cost for a map grid cell
     * @param cx The x coordinate of the cell in the map grid 地图网格中单元格x坐标
     * @param cy The y coordinate of the cell in the map grid 地图网格中单元格y坐标
     * @param path_cost Will be set to the path distance component of the cost function 路径的代价
     * @param goal_cost Will be set to the goal distance component of the cost function 到目标点距离的代价
     * @param occ_cost Will be set to the costmap value of the cell 单元格代价值
     * @param total_cost Will be set to the value of the overall cost function, taking into account the scaling parameters 总体代价，有把尺度因子考虑进去
     * @return True if the cell is traversible and therefore a legal location for the robot to move to 这个单元格可以通过
     */
    bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);

    /**
     * sets new plan and resets state 设置新的全局路径和重置状态
     */
    bool setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan);

private:
    // planner_util是规划器辅助对象， 为局部规划器提供了代价地图，坐标变换，全局规划等算法输入
    base_local_planner::LocalPlannerUtil *planner_util_;
    double stop_time_buffer_; ///< @brief How long before hitting something we're going to enforce that the robot stop 刹车反应时间，避免撞到障碍物
    double path_distance_bias_, goal_distance_bias_, occdist_scale_;
    Eigen::Vector3f vsamples_;
    double sim_period_;///< @brief The number of seconds to use to compute max/min vels for dwa 计算DWA最大/最小速度用的秒数
    base_local_planner::Trajectory result_traj_;
    double forward_point_distance_;
    // 局部路径规划器的参考路径，来至于全局规划，可以可视化
    std::vector<geometry_msgs::PoseStamped> global_plan_;
    boost::mutex configuration_mutex_;
    std::string frame_id_;
    ros::Publisher traj_cloud_pub_;
    bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publish a PointCloud 是否发布点云消息
    bool publish_traj_pc_;
    double cheat_factor_;
    ///< @brief The map grid visualizer for outputting the potential field generated by the cost function 可视化代价函数产生的势场
    base_local_planner::MapGridVisualizer map_viz_;
    // see constructor body for explanations
    base_local_planner::SimpleTrajectoryGenerator generator_;
    // 运动打分器：震荡代价值就会很大
    base_local_planner::OscillationCostFunction oscillation_costs_;
    // 碰撞打分器：发生碰撞代价值就会很大
    base_local_planner::ObstacleCostFunction obstacle_costs_;
    // 路径打分器：轨迹距离局部路径偏差大代价值就会很大
    base_local_planner::MapGridCostFunction path_costs_;
    // 目标打分器：局部轨迹距离局部目标终点远代价值就会很大
    base_local_planner::MapGridCostFunction goal_costs_;
    // 目标点朝向打分器：局部轨迹与局部路径终点方向越偏代价值就会很大
    base_local_planner::MapGridCostFunction goal_front_costs_;
    // 对齐打分器：局部轨迹与局部路径点方向越偏代价值就会很大
    base_local_planner::MapGridCostFunction alignment_costs_;
    // 旋转打分器：旋转速度越大代价值就会很大
    base_local_planner::TwirlingCostFunction twirling_costs_;
    base_local_planner::SimpleScoredSamplingPlanner scored_sampling_planner_;
};
};
#endif
